#ifndef BACKGROUNDSINGLEFRAME_H
#define BACKGROUNDSINGLEFRAME_H

#include "opencv2/core/core.hpp"

#include <stdexcept>
#include <algorithm>
#include <vector>
#include <stdio.h>

#include "BackGroundModel.h"

template< class T>
class BackGroundSingleFrame : public BackGroundModel
{
public:
    BackGroundSingleFrame( int tolerance ,
                           int imgWidth , int imgHeight , int numChannels , bool depthImage ) {

        this->tolerance = tolerance;
        this->imgWidth = imgWidth;
        this->imgHeight = imgHeight;
        this->numChannels = numChannels;
        this->totalElements = imgWidth*imgHeight*numChannels;
        this->depthImage = depthImage;
        this->depth = sizeof(T);

        if( depthImage && numChannels > 1 ) {
            throw std::runtime_error("Expected single channel for depth image");
        }

        bgImg = cv::Mat::zeros(imgHeight, imgWidth, CV_MAKETYPE((depth-1)*2,numChannels));

        reset();
    }

    virtual ~BackGroundSingleFrame() {
    }

    virtual cv::Mat& getBackground() {
        return bgImg;
    }

    virtual void reset() {
    }

    virtual bool addImage( const cv::Mat &image ) {
        if( image.rows != imgHeight )
            throw std::runtime_error("Unexpected number of rows");
        if( image.cols != imgWidth )
            throw std::runtime_error("Unexpected number of columns");
        if( image.channels() != numChannels ) {
             printf("found = %d  expected %d\n",image.channels(),numChannels);
            throw std::runtime_error("Unexpected number of channels");
        }
        // open in opencv is setting the depth to 0
        if( image.depth() > 0 && image.depth() != depth ) {
            printf("image depth = %d  expected %d\n",image.depth(),depth);
            throw std::runtime_error("Unexpected depth");
        }

        image.copyTo(bgImg);

        return true;
    }

    virtual void computeModel() {
    }


    virtual void findMovement( const cv::Mat &image , cv::Mat &movement  ) {
//        if( movement.depth() != 1 )
//            throw std::runtime_error("movement image should have a depth of 1");
//        if( movement.channels() != 1 )
//            throw std::runtime_error("movement image should have 1 channel");

        if( depthImage ) {
            findMovementDepth(image,movement);
        } else {
            findMovementColor(image,movement);
        }
    }

protected:

    void findMovementColor( const cv::Mat &image , cv::Mat &movement  ) {

        const T *srcRowPtr = (const T *)image.datastart;
        uint8_t *dstRowPtr = (uint8_t *)movement.datastart;
        const T *meanRowPtr = (const T *)bgImg.datastart;

        int index = 0;

        for( int i = 0; i < imgHeight; i++ ) {
            const T *srcPtr = srcRowPtr;
            uint8_t *dstPtr = dstRowPtr;
            const T *meanPtr = meanRowPtr;

            for( int j = 0; j < imgWidth; j++ ) {
                bool bad = false;
                for( int k = 0; k < numChannels; k++ , index++ ) {
                    int error = abs( (int)*srcPtr++ - (int)*meanPtr++ );
                    bad |= error > tolerance;
                }
                if( bad ) {
                    *dstPtr++ = 255;
                } else {
                    *dstPtr++ = 0;
                }
            }
            srcRowPtr += image.step/depth;
            dstRowPtr += movement.step;
            meanRowPtr += bgImg.step/depth;
        }
    }

    void findMovementDepth( const cv::Mat &image , cv::Mat &movement  ) {

        const T *srcRowPtr = (const T *)image.datastart;
        uint8_t *dstRowPtr = (uint8_t *)movement.datastart;
        const T *meanRowPtr = (const T *)bgImg.datastart;

        int index = 0;

        for( int i = 0; i < imgHeight; i++ ) {
            const T *srcPtr = srcRowPtr;
            uint8_t *dstPtr = dstRowPtr;
            const T *meanPtr = meanRowPtr;

            for( int j = 0; j < imgWidth; j++ , index++ ) {
                int val = *srcPtr++;

                if( abs( val - (int)*meanPtr++ ) > tolerance && val < 2047 ) {
                    *dstPtr++ = 255;
                } else {
                    *dstPtr++ = 0;
                }
            }
            srcRowPtr += image.step/depth;
            dstRowPtr += movement.step;
            meanRowPtr += bgImg.step/depth;
        }
    }

    int imgWidth;
    int imgHeight;
    int numChannels;
    int depth;

    int totalElements;

    // image with mean values;
    cv::Mat bgImg;
    // tolerance for detecting motion
    int tolerance;
    // is it a depth image or color
    bool depthImage;


};

#endif // BACKGROUNDSINGLEFRAME_H
